#include "robot.h"
#include "robot_state.h"

#include "FreeRTOS.h"
#include "task.h"
#include "spi.h"
#include "dwt.h"


#include "manager.h"
#include "ins_task.h"
#include "kalman_filter.h"


static void robot_task(void *parameter)
{
    TickType_t last_wake_time = xTaskGetTickCount();

    float init_time_counter = 0.0f;

	Robot_State temp_state;
    while(SL_TRUE)
    {
        temp_state = get_robot_state();

        if(temp_state == ROBOT_INIT)
        {
           if(!is_robot_init_done())
           {
               set_robot_state(ROBOT_NORM);
           }
        }

        if(temp_state == ROBOT_IDLE)
        {
            if(!is_soc_2_mcu_init_done() )
            {
                set_robot_state(ROBOT_DISABLE);
            }
            else
            {
                set_robot_state(ROBOT_IDLE);
            }
        }

        if(temp_state == ROBOT_DISABLE && 0)
        {
            set_robot_state(ROBOT_INIT);
        }
		
		INS_Task();
	 
     vTaskDelayUntil(&last_wake_time, ROBOT_TASK_PERIOD_MS);    
    }
	
}


void correspondence_init()
{
    manger_init();
}


void robot_init()
{
    set_robot_state(ROBOT_INIT);

	DWT_Init(168);
	ins_init(&hspi1, 1);
 
	correspondence_init();
	
    xTaskCreate(robot_task, "robot", 1024, NULL, 10, NULL);
}

